#include "SearchToWayPointsState.h"
#include "AStarSearch.h"

SearchToWayPointsState::SearchToWayPointsState(void)
{
	vector<Vector2> way_points;
	follow_way_points_state = new FollowWayPointsState(way_points);
	_initialized = false;
	_done = false;
	cur_waypoint = -1;
}


SearchToWayPointsState::~SearchToWayPointsState(void)
{
	if (follow_way_points_state) {
		delete follow_way_points_state;
	}
}

void SearchToWayPointsState::setNextWayPoint() {
	int MAX_WAYPOINTS = 10;
	if (cur_waypoint < ((int)_waypoints.size() - 1)) {
		cur_waypoint++;
	}
	else {
		cur_waypoint = -1;
		_done = true;
	}
}

void SearchToWayPointsState::add_waypoint_method(Vector2 (*func)(Agent* agent, bool* stateChanged)) {
	_waypoints.push_back(func);
}

Command SearchToWayPointsState::GetCommand(Agent* agent) {
	
	if (!_initialized) {
		add_waypoint_methods();
		_initialized = true;
		setNextWayPoint();
	}

	if (done()) {
		return Command(agent->getTank()->index, 1, .7, true);
	}

	if (follow_way_points_state->done()) {

		World* world = World::instance();
		Graph* graph = world->getGraph();
		tank_t* tank = agent->getTank();
		Vector2 tank_pos(tank->pos[0], tank->pos[1]);
		int size = World::instance()->getSize();
		
		bool stateChanged = false;
		Vector2 target = _waypoints[cur_waypoint](agent, &stateChanged);

		if (stateChanged)
			return agent->GetCommand();

		setNextWayPoint();

		AStarSearch as;

		graph->clearFlags();

		vector<Vector2> path = as.get_search_path(graph, tank_pos, target);

		if (follow_way_points_state) {
			delete follow_way_points_state;
			follow_way_points_state = new FollowWayPointsState(path);
		}
	}
	
	Command cmd = follow_way_points_state->GetCommand(agent);
	cmd.shoot(true);
	return cmd;
}

bool SearchToWayPointsState::done() {
	return _done && follow_way_points_state->done();
}
